/*
 * @Author: czu_xy 3322566362@qq.com
 * @Date: 2025-05-17 18:44:48
 * @LastEditors: czu_xy 3322566362@qq.com
 * @LastEditTime: 2025-05-19 14:36:10
 * @FilePath: \ProjectSTM32F407c:\Users\33225\Desktop\drone\ti-drone-master\ti-drone-master\ANO_LX_FC_F407\FcSrc\wayPoint.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "wayPoint.h"
#include "Drv_UWB.h"
#include "Ano_Math.h"
#include "Drv_AnoOf.h"


wayPointDef qtf_wayPoint[WAY_POINT_NUM];
//航点设置
void wayPointInit(void){
//初始化0，0
    qtf_wayPoint[0].qtfPOintX = 0;
    qtf_wayPoint[0].qtfPOintY = 0;
    qtf_wayPoint[0].qtfPOintZ = 0;

/*任务航点顺序：起点->A -> B -> C -> F -> E -> D ->终点*/

//起飞点
    qtf_wayPoint[1].qtfPOintX = 40;
    qtf_wayPoint[1].qtfPOintY = 40;
    qtf_wayPoint[1].qtfPOintZ = 170;
//降落点
    qtf_wayPoint[2].qtfPOintX = 40;
    qtf_wayPoint[2].qtfPOintY = 40;
    qtf_wayPoint[2].qtfPOintZ = 0;
//任务航点A
    qtf_wayPoint[3].qtfPOintX = 95;
    qtf_wayPoint[3].qtfPOintY = 150;
    qtf_wayPoint[3].qtfPOintZ = 170;
//任务航点B
    qtf_wayPoint[4].qtfPOintX = 260;
    qtf_wayPoint[4].qtfPOintY = 150;
    qtf_wayPoint[4].qtfPOintZ = 170;
//任务航点C
    qtf_wayPoint[5].qtfPOintX = 410;
    qtf_wayPoint[5].qtfPOintY = 140;
    qtf_wayPoint[5].qtfPOintZ = 170;
//任务航点F
    qtf_wayPoint[6].qtfPOintX = 420;
    qtf_wayPoint[6].qtfPOintY = 300;
    qtf_wayPoint[6].qtfPOintZ = 170;
//任务航点E
    qtf_wayPoint[7].qtfPOintX = 270;
    qtf_wayPoint[7].qtfPOintY = 300;
    qtf_wayPoint[7].qtfPOintZ = 170;
//任务航点D
    qtf_wayPoint[8].qtfPOintX = 115;
    qtf_wayPoint[8].qtfPOintY = 300;
    qtf_wayPoint[8].qtfPOintZ = 170;
		
		//强行降落点
    qtf_wayPoint[20].qtfPOintZ = 0;
}
//pid航点更新
void wayPointGetUpdate(u8 wayPointNum){
    
    alt_pid.com_dat.tar_dat = qtf_wayPoint[wayPointNum].qtfPOintZ;
    x_pid.com_dat.tar_dat   = qtf_wayPoint[wayPointNum].qtfPOintX;
    y_pid.com_dat.tar_dat   = qtf_wayPoint[wayPointNum].qtfPOintY;
    yaw_pid.com_dat.tar_dat = qtf_wayPoint[wayPointNum].qtfYAW;

}


u8 wayPointGetCheckX(para_pid_* pidStr,u8 wayPointNum,u8 getCnt,u8 deadZone){
    
    static u8 check_cntx = 0;
    if((pidStr->com_dat.act_dat > qtf_wayPoint[wayPointNum].qtfPOintX - deadZone) 
    && (pidStr->com_dat.act_dat < qtf_wayPoint[wayPointNum].qtfPOintX + deadZone))  
        check_cntx++;
    if(check_cntx > getCnt){
        check_cntx = 0;
        pidStr->pid_st.cmd_flag = 1; //达到标志 
        return(1);
    }
		else return 0;
}
u8 wayPointGetCheckY(para_pid_* pidStr,u8 wayPointNum,u8 getCnt,u8 deadZone){
    
    static u8 check_cnty = 0;
    if((pidStr->com_dat.act_dat > qtf_wayPoint[wayPointNum].qtfPOintY - deadZone) 
    && (pidStr->com_dat.act_dat < qtf_wayPoint[wayPointNum].qtfPOintY + deadZone))  
        check_cnty++;
    if(check_cnty > getCnt){
        check_cnty = 0;
        pidStr->pid_st.cmd_flag = 1; //达到标志 
        return(1);
    }
		else return 0;
}
u8 wayPointGetCheckZ(para_pid_* pidStr,u8 wayPointNum,u8 getCnt,u8 deadZone){
    
    static u8 check_cntz = 0;
    if((pidStr->com_dat.act_dat > qtf_wayPoint[wayPointNum].qtfPOintZ - deadZone) 
    && (pidStr->com_dat.act_dat < qtf_wayPoint[wayPointNum].qtfPOintZ + deadZone))  
        check_cntz++;
    if(check_cntz > getCnt){
        check_cntz = 0;
        pidStr->pid_st.cmd_flag = 1; //达到标志 
        return(1);
    }
		else return 0;
}

